#include "z_test.h"
#include "ZDT_X42_V2.h"
#include <stdbool.h>
#include <stdint.h>
#include "cli_shell.h"
#include "comm/ZDT_X42_V2.h"

#define DBG_TAG "ZT"
#define DBG_LVL DBG_LOG
#include <dbg_print.h>
#include "systick.h" 							// 包括有sys_tick 时间戳 
//Z轴移动状态
#define Z_MOVE_STATE_IDLE           0
#define Z_MOVE_STATE_HOME           1
#define Z_MOVE_STATE_DIST1          2 
#define Z_MOVE_STATE_DIST2          3
#define Z_MOVE_STATE_END            4

//移动方向
#define Z_TEST_MOVE_OPT_UP          1
#define Z_TEST_MOVE_OPT_DOWN        2
#define Z_TEST_MOVE_OPT_UP_WAIT     3
#define Z_TEST_MOVE_OPT_DOWN_WAIT   4

//1秒计数值
#define Z_TEST_SECOND               100
#define Z_TEST_MOVE_WAIT_TIME       100

//error code
#define ERROR_CODE_NONE             0
#define ERROR_CODE_TIMEOUT          1
#define ERROR_CODE_CLOG             2


static uint32_t z_move_timeout = 0;
static uint8_t tst_z_move_state = Z_MOVE_STATE_IDLE;
static uint8_t tst_z_move_opt = 0;
static uint8_t tst_z_move_cnt = 0;
static uint8_t rxCmd[128] = {0}; 
static uint8_t rxCount = 0;
static uint8_t error_code = 0;


//LED1:PB0
#define LED1_GPIO_CLK               RCU_GPIOB
#define LED1_GPIO                   GPIOB
#define LED1_PIN                    GPIO_PIN_0
//LED2:PA12
#define LED2_GPIO_CLK               RCU_GPIOA
#define LED2_GPIO                   GPIOA
#define LED2_PIN                    GPIO_PIN_12

#define LED_RED_ON()                GPIO_BOP(LED2_GPIO) = LED2_PIN
#define LED_RED_OFF()               GPIO_BC(LED2_GPIO) = LED2_PIN
#define LED_GREEN_ON()               GPIO_BOP(LED1_GPIO) = LED1_PIN
#define LED_GREEN_OFF()              GPIO_BC(LED1_GPIO) = LED1_PIN


//电机使能控制：电机发转动命令前需使能
uint8_t tst_z_move_enable(uint8_t state)
{
    ZDT_X42_V2_En_Control(1,1,0);
    ZDT_X42_V2_Receive_Data(rxCmd, &rxCount);
    return ZDT_X42_V2_Check_ACK(rxCmd, rxCount);
}

//电机立即停止
uint8_t tst_z_move_stop(void)
{
    ZDT_X42_V2_Stop_Now(1,0);
    ZDT_X42_V2_Receive_Data(rxCmd, &rxCount);
    return ZDT_X42_V2_Check_ACK(rxCmd, rxCount);
}

//当前的位置角度清零
uint8_t tst_z_move_pos_clear(void)
{
    ZDT_X42_V2_Pos_Clear(1);
    ZDT_X42_V2_Receive_Data(rxCmd, &rxCount);
    return ZDT_X42_V2_Check_ACK(rxCmd, rxCount);
}

//解除堵转保护（复位堵保护）
uint8_t tst_z_reset_clog_pro(void)
{
    LOG_D("reset clog!");
    ZDT_X42_V2_Reset_Clog_Pro(1);
    ZDT_X42_V2_Receive_Data(rxCmd, &rxCount);
    return ZDT_X42_V2_Check_ACK(rxCmd, rxCount);
}


//直通限速位置模式控制
uint8_t tst_z_move_pos_set(uint8_t dir, uint32_t pos)
{
    uint32_t pos_angle = DIST_TO_CNT(pos);
    LOG_D("%s dir:%d,dist:%dmm(%d.%d)", __FUNCTION__, dir, pos, pos_angle/10, ABS(pos_angle)%10);
    //void ZDT_X42_V2_Bypass_Position_Control(uint8_t addr, uint8_t dir, uint32_t velocity, uint32_t position, uint8_t raf, uint8_t snF);
    //直通限速位置模式控制
    ZDT_X42_V2_Bypass_Position_Control(1, dir, MOVE_VELOCITY, pos_angle,1,0);
    ZDT_X42_V2_Receive_Data(rxCmd, &rxCount);
    LOG_HEX("s:",16,rxCmd, rxCount);
    return ZDT_X42_V2_Check_ACK(rxCmd, rxCount);
}

//读到电机系统参数 
uint8_t tst_z_move_state_get(struct motor_state *state)
{
    ZDT_X42_V2_Read_Sys_Params(1,S_State);
    // 等待返回命令，命令数据缓存在数组rxCmd上，长度为rxCount
    ZDT_X42_V2_Receive_Data(rxCmd, &rxCount);
    if ((rxCmd[1] == 0x43) && (rxCount == 37) && (rxCmd[rxCount - 1] == 0x6B))
    {
        ZDT_X42_V2_Motor_State_Unpack(rxCmd, state);
        //LOG_HEX("s:",16,rxCmd, rxCount);
        return 1;
    }
    return 0;
}

//回零参数检测
uint8_t tst_z_move_org_get(struct org_param *param)
{
    ZDT_X42_V2_Read_Sys_Params(1,S_ORG);
    // 等待返回命令，命令数据缓存在数组rxCmd上，长度为rxCount
    ZDT_X42_V2_Receive_Data(rxCmd, &rxCount);
    LOG_HEX("ORGs:",16,rxCmd, rxCount);
	if ((rxCmd[1] == 0x22) && (rxCount == 18) && (rxCmd[rxCount - 1] == 0x6B)) // 0x6B表示校验位
    {
        ZDT_X42_V2_ORG_Param_Unpack(rxCmd, param);
        //LOG_HEX("s:",16,rxCmd, rxCount);
        return 1;
    }
    return 0;
}

//回零状态检测
uint8_t tst_z_move_org_is_idle(void)
{
    ZDT_X42_V2_Read_Sys_Params(1,S_OFLAG);
    // 等待返回命令，命令数据缓存在数组rxCmd上，长度为rxCount
    ZDT_X42_V2_Receive_Data(rxCmd, &rxCount);
    if ((rxCmd[1] == 0x3B) && (rxCount == 4) && (rxCmd[rxCount - 1] == 0x6B))
    {
        //ZDT_X42_V2_ORG_Param_Unpack(rxCmd, param);
        //LOG_HEX("s:",16,rxCmd, rxCount);
        if (rxCmd[2] != 0xEE)
        {
            if ((rxCmd[2] & READY_STATE_HOMING) == 0)
            {
                return 1;
            }
        }
    }
    return 0;
}

//回零初始化，未配置，则配置：多圈无限位碰撞回零
uint8_t tst_z_move_org_init(void)
{
    struct org_param param;

    if (tst_z_move_org_get(&param))  
    {
        //LOG_D("org get");
        LOG_D("mode:%d,dir:%d,rpm:%d,timeout:%d,c_rpm:%d,c_cur:%d,c_time:%d,pwr_en:%d",
            param.mode,param.dir,param.rpm,param.timeout,param.collision_rpm,
            param.collision_current,param.collision_time,param.power_on_en);
        if ((param.mode != HOMING_MODE) || (param.power_on_en == false) ||
            (param.rpm != HOMING_VELOCITY) ||
            (param.collision_current != HOMING_ENDSTOP_CURRENT) || 
            (param.timeout != 1000*MOVE_TIMEOUT_HOME))
        {
            LOG_D("origin modify params");
            ZDT_X42_V2_Origin_Modify_Params(1, true, HOMING_MODE, DIR_HOME_DOWN, 
                HOMING_VELOCITY, 1000*MOVE_TIMEOUT_HOME,300, 
                HOMING_ENDSTOP_CURRENT,HOMING_ENDSTOP_TIME,true);       
            ZDT_X42_V2_Receive_Data(rxCmd, &rxCount);
            return ZDT_X42_V2_Check_ACK(rxCmd, rxCount);
        }
    }
    return 0;
}

//开始无限位回零
uint8_t tst_z_move_org_start(void)
{
    LOG_D("%s", __FUNCTION__);
	/**
  * @brief    触发回零
  * @param    addr   ：电机地址
  * @param    o_mode ：回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零
  * @param    snF    ：多机同步标志，false为不启用，true为启用
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
    ZDT_X42_V2_Origin_Trigger_Return(1,2,0);
    ZDT_X42_V2_Receive_Data(rxCmd, &rxCount);
    return ZDT_X42_V2_Check_ACK(rxCmd, rxCount);
}

void tst_z_test_init(void)
{
    /* enable the led clock */
    rcu_periph_clock_enable(LED1_GPIO_CLK);
    /* configure led GPIO port */ 
    gpio_init(LED1_GPIO, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ,LED1_PIN);
    GPIO_BC(LED1_GPIO) = LED1_PIN;//clear

    /* enable the led clock */
    rcu_periph_clock_enable(LED2_GPIO_CLK);
    /* configure led GPIO port */ 
    gpio_init(LED2_GPIO, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ,LED2_PIN);
    GPIO_BC(LED2_GPIO) = LED2_PIN;//clear
}

void tst_z_move_start(void)
{
    LOG_D("%s speed :%d\n", __FUNCTION__, MOVE_VELOCITY);

    //tst_z_move_enable = 1;
	tst_z_move_state = Z_MOVE_STATE_HOME; // 设置状态为回零， 
    tst_z_move_opt = Z_TEST_MOVE_OPT_DOWN; // 设置操作向下移动 
    if (tst_z_move_pos_clear())
    {
        LOG_D("position clear start");
    }
    if (!tst_z_move_enable(1))
    {
        if (!tst_z_move_enable(1))
        {
            LOG_D("move enable error!");
        }
    }
    if (!tst_z_move_org_init())
    {
        //重发
        if (!tst_z_move_org_init())
        {
            LOG_D("move org init error!");
        }
    }
    if (!tst_z_move_org_start())  // 触发回零
    {
        if (!tst_z_move_org_start())
        {
            LOG_D("move org start error!");
        }
    }
    z_move_timeout = Z_TEST_SECOND*MOVE_TIMEOUT_HOME+Z_TEST_SECOND;
    error_code = 0;
    LED_GREEN_OFF();
    LED_RED_OFF();
}

void tst_z_move_task(void)
{
    //uint32_t i = 0;
    static struct motor_state state;      
    static uint32_t query_timeout = 10;
    //static uint8_t stop_flag = 0;
		static uint8_t endstop = 0;				// 标定各个移动结束，当为1时， 就进入
    static uint8_t current_cnt = 0;   // 来回次数
    static uint16_t current[10] = {0};
		static uint32_t tick =0 ;  				// 相当于时间戳 
		static  uint8_t belowflag =  0 ;  // 小于的话就有这个标志
		static uint32_t last_tick = 0 ;   // 上一次的计时 
    if (query_timeout > 0)             
        query_timeout--;

    if (z_move_timeout > 0)
        z_move_timeout--;

    if (tst_z_move_state == Z_MOVE_STATE_IDLE)   //如果是空闲状态 ，则将current_cnt设置为0 
    {
        current_cnt = 0;          							 
        return;
    }

    if (query_timeout == 0)   // 查询状态                    
    {
        query_timeout = Z_TEST_SECOND / 50;   //查询时间,设置每20ms进行一次查询
		if (tst_z_move_state_get(&state))     	//获取电机状态
        {
            if ((tst_z_move_state == Z_MOVE_STATE_DIST1) || (tst_z_move_state == Z_MOVE_STATE_DIST2))  // 在测试状态
            {
                if (current_cnt >= 10)
                {
										belowflag = 0 ;  // 重设值 belowflag 
                    current_cnt = 0;
                    LOG_D("RPM:%d.%d", state.real_rpm/10, ABS(state.real_rpm)%10);

										
                    for(int i = 0 ; i < 10 ; ++ i )
                    {
											if (current[i] <  CURRENT_THREAD) // 如果小于 设定值 那么就置位
											{
												belowflag = 1;  // 只要有一个小于设定值，那么就说明不是全部大于设定值
											}
                    }
                    if(belowflag == 0) // 如果都大于 CURRENT_THREAD ,就亮灯
                    {
                        LED_RED_ON() ;
												LOG_D("red on \r\n");
                    }
									}
                else 
                {

                    current[current_cnt++] = state.current;
                }
									tick = tick +1 ; 
								LOG_D("time=%d current=%d\r\n",tick  , state.current) ; 
            }
            else if (tst_z_move_state == Z_MOVE_STATE_HOME) // 如果是归零状态  查询 每10秒打印一次RPM和POSITION
            {
                static uint16_t home_sec = 0; 
                home_sec++;
                if (home_sec > 10)
                {
                    home_sec = 0;
                    LOG_D("RPM=%d,POSITION:%d",state.real_rpm/10,state.real_pos/10);
                }
            }
            if (state.motor_flag & (MOTOR_STATE_STALL|MOTOR_STATE_STALL_PROTECT))   //堵转
            {
                if (state.motor_flag & MOTOR_STATE_ENABLE)
                {
                    LOG_D("motor stalling");
                    if (tst_z_move_stop())
                    {
                        LOG_D("motor stop ok!");
                    }
                    if (tst_z_move_enable(0))
                    {
                        LOG_D("motor disable!");
                    }
                    if ((tst_z_move_state == Z_MOVE_STATE_DIST1) || 
                        (tst_z_move_state == Z_MOVE_STATE_DIST2) ||
                        ((tst_z_move_state == Z_MOVE_STATE_HOME) && (tst_z_move_opt == Z_TEST_MOVE_OPT_DOWN_WAIT)))
                    //if (tst_z_move_state != Z_MOVE_STATE_END)
                    {
                        LOG_D("clog:tst_z_move_state=%d", tst_z_move_state);
                        tst_z_reset_clog_pro(); // 解除堵转
                        tst_z_move_state = Z_MOVE_STATE_END;
                        error_code = ERROR_CODE_CLOG;                        
                    }

                }
            }
            if (state.motor_flag & MOTOR_STATE_ENDSTOP) //到位
            {
                if (!endstop)
                {
                    endstop = 1;   						//达到末尾
                    LOG_D("motor endstop!T_pos=%d.%d,R_pos=%d.%d,ready_flag=%02x,motor_flag=%02x",
                        state.target_pos/10, ABS(state.target_pos)%10, 
                        state.real_pos/10, ABS(state.real_pos)%10,
                        state.ready_flag, state.motor_flag);
                }
            }
            else {
                endstop = 0;
            }
        }
    }

    if (tst_z_move_state == Z_MOVE_STATE_HOME)  	//进行回零 
    {
        if (tst_z_move_opt == Z_TEST_MOVE_OPT_DOWN) // 如果是向下移动
        {
            if (tst_z_move_org_is_idle())			// 回零状态检测 已回零 
            {
                LOG_D("homing finish!position=%d.%d", state.real_pos/10, ABS(state.real_pos)%10);
                z_move_timeout = 100;
                while (z_move_timeout--) 			// 在1s时间内查看是否将当前的位置角度清零 
                {
                    //将当前的位置角度清零，后续的上下移动距离的依据
                    if (tst_z_move_pos_clear())
                    {
                        if (tst_z_move_state_get(&state))
                        {
                            if (state.real_pos == 0)
                            {
								LOG_D("real position clear");
								tst_z_move_opt = Z_TEST_MOVE_OPT_DOWN_WAIT;
								z_move_timeout = Z_TEST_MOVE_WAIT_TIME;
								break;
                            }
                        }
                    }
                        delay_1ms(10);
                }
                if (z_move_timeout == 0) 					   //当超时后，打印日志
                {
                    LOG_D("position clear timeout!");
                    tst_z_move_state = Z_MOVE_STATE_END;
                    error_code = ERROR_CODE_TIMEOUT;
                    return;
                }
            }
            if (z_move_timeout == 0)   						  //  回零超时
            {
                LOG_D("homing timeout!");
                tst_z_move_state = Z_MOVE_STATE_END;
                error_code = ERROR_CODE_TIMEOUT;
            }
        }
        else if (tst_z_move_opt == Z_TEST_MOVE_OPT_DOWN_WAIT) //下降等待状态
        {
            if (z_move_timeout == 0)
            {
                //Z轴热床开始上升 间距1 并状态转移到DIST1
                if (tst_z_move_pos_set(DIR_HOME_UP,TEST_MOVE_DISTANCE1))  
                {
                    LOG_D("switch to state_dist1(%dmm)", TEST_MOVE_DISTANCE1);
                    LOG_D("DIST1_ UP...");
                    tst_z_move_state = Z_MOVE_STATE_DIST1;
                    tst_z_move_opt = Z_TEST_MOVE_OPT_UP;
                    z_move_timeout = MOVE_TIMEOUT_DIST1 * Z_TEST_SECOND;
                    endstop = 0;  													
                    tst_z_move_cnt = 0;
                }
//						tst_z_move_state = Z_MOVE_STATE_END;
								
            }
        }
    }
    else if (tst_z_move_state == Z_MOVE_STATE_DIST1)    // 不是回零状态 而是以间距1来回运动状态
    {
        if (tst_z_move_opt == Z_TEST_MOVE_OPT_DOWN)     // 向下移动 ，等待其完成 
        {
			if (endstop)  															
            {
                //到位
                LOG_D("DIST1_DOWN ENDSTOP");
                if (tst_z_move_stop())
                {
                    tst_z_move_opt = Z_TEST_MOVE_OPT_DOWN_WAIT;
                    z_move_timeout = Z_TEST_MOVE_WAIT_TIME;
                }
            }
            else if (z_move_timeout == 0)               
            {
                LOG_D("move timeout:switch to state_end");
                tst_z_move_state = Z_MOVE_STATE_END;	 
                error_code = ERROR_CODE_TIMEOUT;
            }
        }
        else if (tst_z_move_opt == Z_TEST_MOVE_OPT_UP)  // 向上移动，等待其完成  
        {
            if (endstop)
            {
                //到位
                LOG_D("DIST1_UP ENDSTOP");
                if (tst_z_move_stop())
                {
					tst_z_move_opt = Z_TEST_MOVE_OPT_UP_WAIT;  // 向上移动到位之后是 等待状态
                    z_move_timeout = Z_TEST_MOVE_WAIT_TIME;
                }
            }
            else if (z_move_timeout == 0)
            {
                LOG_D("move timeout:switch to state_end");
                tst_z_move_state = Z_MOVE_STATE_END;
                error_code = ERROR_CODE_TIMEOUT;
            }
        }
        else if (tst_z_move_opt == Z_TEST_MOVE_OPT_UP_WAIT)  // 向上 等待移动 
        {
            if (z_move_timeout == 0)
            {
							//Z轴热床开始下降 到回零点缩回处
                if (tst_z_move_pos_set(DIR_HOME_UP,HOMING_RETRACT_DIST))
                {
                    LOG_D("DIST1_DOWN...");
                    tst_z_move_opt = Z_TEST_MOVE_OPT_DOWN;     // 标记处于下降状态
                    z_move_timeout = MOVE_TIMEOUT_DIST1 * Z_TEST_SECOND;
                    endstop = 0; // 在移动过程中没有达到尾
                }
            }
        }
        else if (tst_z_move_opt == Z_TEST_MOVE_OPT_DOWN_WAIT)  // 否则下降等待状态 ， 在该状态下进行上升 
        {
            if (z_move_timeout == 0) 
            {
                tst_z_move_cnt++;
                if (tst_z_move_cnt >= TEST_MOVE_NUM1)
                {
                    if (TEST_MOVE_NUM2 == 0)
                    {
                        tst_z_move_state = Z_MOVE_STATE_END;
                        return;
                    }
                    //切换到间距2来回运动状态
                    if (tst_z_move_pos_set(DIR_HOME_UP,TEST_MOVE_DISTANCE2))
                    {
                        LOG_D("switch to state_dist2(%dmm)", TEST_MOVE_DISTANCE2);
                        LOG_D("DIST2_UP...");
                        tst_z_move_opt = Z_TEST_MOVE_OPT_UP;
                        z_move_timeout = MOVE_TIMEOUT_DIST2 * Z_TEST_SECOND;    // z移动超时
                        endstop = 0;	// 在移动过程中没有达到尾
                        tst_z_move_cnt = 0;
                        tst_z_move_state = Z_MOVE_STATE_DIST2;
                    }
                }
                else 
                {
                    //Z轴热床开始上升到间距1
                    if (tst_z_move_pos_set(DIR_HOME_UP,TEST_MOVE_DISTANCE1))
                    {
                        LOG_D("DIST1_UP...");
                        tst_z_move_opt = Z_TEST_MOVE_OPT_UP;
                        z_move_timeout = MOVE_TIMEOUT_DIST1 * Z_TEST_SECOND;    // 
                        endstop = 0;  // 在移动过程中没有达到尾
                    }
                }
            }
        }
    }
    else if (tst_z_move_state == Z_MOVE_STATE_DIST2)    //以间距2来回运动状态
    {
        if (tst_z_move_opt == Z_TEST_MOVE_OPT_DOWN)
        {
            if (endstop)
            {
                //到位
                LOG_D("DIST2_DOWN ENDSTOP");
                if (tst_z_move_stop())
                {
                    tst_z_move_opt = Z_TEST_MOVE_OPT_DOWN_WAIT;
                    z_move_timeout = Z_TEST_MOVE_WAIT_TIME;
                }
            }
            else if (z_move_timeout == 0)
            {
                LOG_D("move timeout:switch to state_end");
                tst_z_move_state = Z_MOVE_STATE_END;
                error_code = ERROR_CODE_TIMEOUT;
            }
        }
        else if (tst_z_move_opt == Z_TEST_MOVE_OPT_UP)
        {
            if (endstop)
            {
                //到位
                LOG_D("DIST2_UP ENDSTOP");
                if (tst_z_move_stop())
                {
                    tst_z_move_opt = Z_TEST_MOVE_OPT_UP_WAIT;
                    z_move_timeout = Z_TEST_MOVE_WAIT_TIME;
                }
            }
            else if (z_move_timeout == 0)
            {
                LOG_D("move timeout:switch to state_end");
                tst_z_move_state = Z_MOVE_STATE_END;
                error_code = ERROR_CODE_TIMEOUT;
            }
        }
        else if (tst_z_move_opt == Z_TEST_MOVE_OPT_UP_WAIT)  
        {
            if (z_move_timeout == 0)
            {
                //Z轴热床开始下降到回零点缩回处
                if (tst_z_move_pos_set(DIR_HOME_UP,HOMING_RETRACT_DIST))
                {
                    LOG_D("DIST2_DOWN...");
                    tst_z_move_opt = Z_TEST_MOVE_OPT_DOWN;
                    z_move_timeout = MOVE_TIMEOUT_DIST2 * Z_TEST_SECOND;
                    endstop = 0;
                }
            }
        }
        else if (tst_z_move_opt == Z_TEST_MOVE_OPT_DOWN_WAIT)
        {
            if (z_move_timeout == 0)
            {
                tst_z_move_cnt++;
                if (tst_z_move_cnt >= TEST_MOVE_NUM2)
                {
                    LOG_D("switch to state_end");
                    tst_z_move_state = Z_MOVE_STATE_END;
                }
                else 
                {
                    //Z轴热床开始上升到间距1
                    if (tst_z_move_pos_set(DIR_HOME_UP,TEST_MOVE_DISTANCE2))
                    {
                        LOG_D("DIST2_UP...");
                        tst_z_move_opt = Z_TEST_MOVE_OPT_UP;
                        z_move_timeout = MOVE_TIMEOUT_DIST2 * Z_TEST_SECOND;
                        endstop = 0;
                    }
                }
            }
        }
    }
    else if (tst_z_move_state == Z_MOVE_STATE_END) // 既不是回零状态，也不是以间距1或者间距2运动的状态，也不是 运动过程中的查询状态 
    {
        tst_z_move_cnt = 0;
        tst_z_move_state = Z_MOVE_STATE_IDLE;
        if (tst_z_move_stop())
        {
            LOG_D("motor stop ok!");
        }
        if (tst_z_move_enable(0))
        {
            LOG_D("motor disable!");
        }
        LOG_D("switch to state_idle\n");
        if (error_code == 0)
        {
            LED_GREEN_ON();
        }
        else {
            LED_RED_ON();
        }
				tick = 0 ; 
    }
}


/*************************************命令行功能 *************************************/
static int move_clear(int argc, char **argv)
{
    if (tst_z_move_pos_clear())
    {
        LOG_D("move position clear ok");
    }
    return 0;
}
CLI_CMD_EXPORT(move_clear, move clear);

static int move_start(int argc, char **argv)
{
    tst_z_move_start();
    return 0;
}
CLI_CMD_EXPORT(move_start, move_start);

static int move_state(int argc, char **argv)
{
    static struct motor_state state;
    if (tst_z_move_state_get(&state))
    {
        LOG_D("state>> vol=%dmv,cur=%dma,pos=%d, motor_flag=%02x, vel=%drpm\n", 
            state.voltage, state.current, state.real_pos, state.motor_flag, state.real_rpm);
        LOG_D("target_pos=%d,real_pos=%d", state.target_pos, state.real_pos);
    }
    return 0;
}
CLI_CMD_EXPORT(move_state, move_state);

static int move_pos(int argc, char **argv)
{
    uint32_t pos;
    uint8_t dir;
    if (argc > 2)
    {
        dir = atoi(argv[1]);
        pos = atoi(argv[2]);
        if (pos > 500)
            pos = 500;
        if (tst_z_move_pos_set(dir,pos))
        {
            LOG_D("dir:%d,position set:%d ...", dir, pos);
        }
    }
    else {
        LOG_D("Usage: \n");
        LOG_D("move_pos [dir] [pos]");
    }

    return 0;
}
CLI_CMD_EXPORT(move_pos, move_pos [dir] [pos]);


static int move_stop(int argc, char **argv)
{
	if (tst_z_move_stop()  == 1) // 直到返回1 表示停止
	{
		LOG_D("motor stop success\r\n") ;
	}
	else
	{
		LOG_D("motor stop failed\r\n") ; 
	}
	return 0 ; 
}
CLI_CMD_EXPORT(move_stop, move_stop );

static int move_org(int argc, char **argv)
{
    struct org_param param;

    if (argc > 1)
    {
        if (!strcmp(argv[1], "get"))
        {
            if (tst_z_move_org_get(&param))
            {
                //LOG_D("org get");
                LOG_D("mode:%d,dir:%d,rpm:%d,timeout:%d,c_rpm:%d,c_cur:%d,c_time:%d,pwr_en:%d",
                    param.mode,param.dir,param.rpm,param.timeout,param.collision_rpm,
                    param.collision_current,param.collision_time,param.power_on_en);
            }
        }
        if (!strcmp(argv[1], "init"))
        {
            if (tst_z_move_org_init())
            {
                LOG_D("org init ok");
            }
        }
        if (!strcmp(argv[1], "idle"))
        {
            if (tst_z_move_org_is_idle())
            {
                LOG_D("org is idle");
            }
        }
    }
    return 0;
}
CLI_CMD_EXPORT(move_org, move_org [get/init]);